/*
 * Copyright The Slam Authors
 */

#ifndef SLAM_MAPPING_REFLECTOR_UTILITIES_H_
#define SLAM_MAPPING_REFLECTOR_UTILITIES_H_

#include "slam/mapping/pose_graph_interface.h"
#include "slam/sensor/landmark_data.h"
#include "slam/sensor/point_cloud.h"
#include "slam/transform/rigid_transform.h"
#include <deque>

namespace slam {
namespace mapping {

struct LandmarkObservation {  
  int node_id;
  int landmark_id;
  Eigen::Vector2d p_tracking_landmark;
};

struct Landmark {
  int landmark_id = -1;
  int num_node_not_observed_ = 0;
  std::vector<LandmarkObservation> observations;
  Eigen::Vector2d p_local_landmark;
};

struct LocalNode {
  int node_id;
  transform::Rigid2d T_local_tracking;
  std::vector<LandmarkObservation> observations;
};

typedef std::shared_ptr<LocalNode> LocalNodePtr;
typedef std::shared_ptr<Landmark> LandmarkPtr;

void ClusterPoints(const sensor::PointCloud& points, const float threshold,
                   sensor::PointCloud* centers_ptr,
                   std::vector<int>* count_ptr);

void ExtractReflectorFeature(const sensor::PointCloud& reflector_raw_data,
                             const int min_count,
                             sensor::PointCloud* reflector_data_ptr);

void ReflectorMatching(
    const std::map<int, LandmarkPtr>& reflectors_map,
    const transform::Rigid2d& T_local_tracking,
    const sensor::PointCloud& reflector_feature_in_tracking,
    const float match_distance_threshold,
    std::vector<int>* match_reflector_ids_ptr,
    std::vector<double>* match_min_dists_ptr);

void ReflectorMatching(
    const std::map<std::string, Eigen::Vector3d>& reflectors_nearby,
    const transform::Rigid3d& T_global_tracking,
    const sensor::PointCloud& reflector_feature_in_tracking,
    const float match_distance_threshold,
    std::vector<std::string>* match_reflector_ids_ptr,
    std::vector<double>* match_min_dists_ptr);
}  // namespace mapping
}  // namespace slam

#endif  // SLAM_MAPPING_DETECT_FLOORS_H_
